#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#一条规划，回到middle位置再送食物，会掉电，所以延时后回到middle
import sys
import rospy
import moveit_commander
import geometry_msgs.msg
from moveit_commander import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from std_msgs.msg import Int32
import pathall

def bowl_number_callback(msg):
    rospy.loginfo(f"Received bowl number: {msg.data}")
    if msg.data == 1:
        pathall.execute_tra3()
    if msg.data == 2:
        pathall.execute_tra2()
    if msg.data == 3:
        pathall.execute_tra1()

def listener():
    rospy.init_node('bowl_number_listener', anonymous=True)

    # 初始化机械臂控制对象
    moveit_commander.roscpp_initialize(sys.argv)
    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()

    # 机械臂控制群组
    arm_group = moveit_commander.MoveGroupCommander("manipulator")

    # 将机械臂移动到初始位置
    rospy.loginfo("Moving arm to initial position 'seefood'")
    arm_group.set_named_target("seefood")
    plan = arm_group.go(wait=True)
    rospy.loginfo("可以开始识别事物")

    # 确保机械臂成功移动
    arm_group.stop()
    arm_group.clear_pose_targets()
    rospy.Subscriber("/bowl_number", Int32, bowl_number_callback)
    rospy.spin()

if __name__ == '__main__':
    try:
        listener()
    except rospy.ROSInterruptException:
        pass
